/********************************************************************************
*  Project   		: Neurologist
*  File Name  		: Neurologist.cpp        
*  Contributors 	: 599 Code Monkeys
*  Creation Date 	: Jan. 17, 2009
*  Revision History	: Source code & revision history maintained at 10.101.29.110 
*  File Description	: Implementation of Robodox 2009 FRC robot, the Neurologist.
* 					  Contains drive and manipulator control functions along with
* 					  autonomous camera tracking
*/                         
/*----------------------------------------------------------------------------*/
/*        Copyright (c) FIRST 2008.  All Rights Reserved.                     */
/*  Open Source Software - may be modified and shared by FRC teams. The code  */
/*  must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
/*
#include <iostream.h>
//#include <cstdlib>
//#include <time.h>
#include "math.h"

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h" 
#include "Target.h" 
#include "WPILib.h"
#include "RobodoxTimer.h"
#include "NeuroAuto.hh"
//#include "RobodoxEncoder.h"
#include "Neurologist.h"
#include "DriverStationLCD.h"
*/
#include "Neurologist.h"

// To locally enable debug printing: set VisionDemo_debugFlag to a 1, to disable set to 0
int Neurologist_debugFlag = 0;
#define DPRINTF if(Neurologist_debugFlag)dprintf

#define PI (3.14159265358979)

#define UPDATE_TIME_SECONDS (0.05)

// for 160x120, 50 pixels = .38 %
#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 20.0		// target is too close

static const float gearRatio = 12.5;			//Andy Mark toughbox ratio
static const float Ke = 0.021142234;			//CIM-001 Motor back EMF constant in volts/(radian/second)
static float maxWheelRate;

/** Simple test to see if the color is taking up too much of the image */
int tooClose(ParticleAnalysisReport* par) {
	if (par->particleToImagePercent > MAX_PARTICLE_TO_IMAGE_PERCENT)
		return 1;
	return 0;
}

/** Simple test to see if the color is large enough */
int bigEnough(ParticleAnalysisReport* par) {
	if (par->particleToImagePercent < MIN_PARTICLE_TO_IMAGE_PERCENT)
		return 0;
	return 1;
}

static int Update(Neurologist *neuro ){
	float headingLast = 0;
	float dist_l = 0.0;
	float dist_r = 0.0;
	float dist_lref = 0.0;
	float dist_rref = 0.0;
	float dist_l_last = 0.0;
	float dist_r_last = 0.0;
	float dist_lref_last = 0;
	float dist_rref_last = 0;
	float velocityXlast = 0;
	float velocityYlast = 0;
		
	while(1){
		//now save the last values
		dist_l_last = dist_l;
		dist_r_last = dist_r;	
		dist_lref_last = dist_lref;
		dist_rref_last = dist_rref;	
		
		dist_l =  neuro->leftGearbox->GetDistance();
		dist_r = neuro->rightGearbox->GetDistance();
		dist_rref = neuro->rightRef->GetDistance();
		dist_lref = neuro->leftRef->GetDistance();
		
		neuro->v_l = (dist_l - dist_l_last)/(UPDATE_TIME_SECONDS); 
		neuro->v_r = (dist_r - dist_r_last)/(UPDATE_TIME_SECONDS); 
		neuro->v_lref = (dist_lref-dist_lref_last)/(UPDATE_TIME_SECONDS);
		neuro->v_rref = (dist_rref-dist_rref_last)/(UPDATE_TIME_SECONDS);
			
		//Compute motor speed for traction controler
		neuro->l_rate_rps = neuro->v_l/neuro->w_radius;
		neuro->r_rate_rps = neuro->v_r/neuro->w_radius;
				
		//Compute average values for use within other computations.
		neuro->Mr_Robauto->v_robot = (neuro->v_lref + neuro->v_rref)/2;
		neuro->v_avg = (neuro->v_l + neuro->v_r) /2;
				
		neuro->accelerationX = neuro->a_x->GetAcceleration();
		neuro->accelerationY = neuro->a_y->GetAcceleration();
				
		// needs to be fixed so constant acceleration != 0 velocity
		neuro->velocityX = velocityXlast + (neuro->accelerationX*UPDATE_TIME_SECONDS);
		neuro->velocityY = velocityYlast + (neuro->accelerationY*UPDATE_TIME_SECONDS);		
		velocityXlast = neuro->velocityX;
		velocityYlast = neuro->velocityY;
			
		// TODO: Create own Gyro Rate function
		//headingLast = neuro->heading;
		//neuro->heading = neuro->g1->GetAngle();
		//neuro->headingRate = (neuro->heading - headingLast)/(UPDATE_TIME_SECONDS);
			
		Wait(UPDATE_TIME_SECONDS);
	}
	
	return 0;
}

/*
class Neurologist : public SimpleRobot
{
	//RobotDrive *myRobot;			// robot drive system
	Joystick *driveStick;			// joystick 1 (drive)
	Joystick *liftStick;			// joystick 2 (roller)
	DriverStation *ds;				// driver station object
	DriverStationLCD *dsLCD;		// Driver Station LCD object
	Servo *horizontalServo;  		// first servo object
	Servo *verticalServo;			// second servo object
	
	Jaguar *leftMotor;
	Jaguar *rightMotor;
	Relay *liftR;
	Relay *liftL; 
	Relay *agitator;
	Victor *rollerL;
	Victor *rollerR;
	
	
	RobodoxTimer *time;
	

	float horizontalDestination;
	float verticalDestination;
	float horizontalPosition, verticalPosition;	
	float horizontalServoPosition, verticalServoPosition;
	float servoDeadband;			// percentage servo delta to trigger move
	int framesPerSecond;			// number of camera frames to get per second
	float panControl;				// to slow down pan
	float panPosition;
	double sinStart;
	static double panStartTime;
	
	TrackingThreshold td1, td2;		// color thresholds
	ParticleAnalysisReport par1, par2;	// particle analysis report
	ColorReport cReport, cReport2;		// color report	     
	
	

public:
	
	NeuroAuto * Mr_Robauto;
	Gyro *g1;
	Accelerometer *a_x;
	Accelerometer *a_y;
	//Accelerometer *a_z;
	DigitalInput *toplimitswitch;
	DigitalInput *bottomlimitswitch;
	
	DigitalInput *autoswitch1;
	DigitalInput *autoswitch2;
	
	
	RobodoxEncoder *leftGearbox;
	RobodoxEncoder *rightGearbox;
	RobodoxEncoder *rightRef;
	RobodoxEncoder *leftRef;
	
	Task updateTask;
	
	
	float xt, yt;
	float leftMotorCmd;
	float rightMotorCmd;
	float lastRightMotorCmd;
	float lastLeftMotorCmd;
	
	float v_battery;
	
	static const float w_radius = 3.0;  //main wheel radius
	static const float ref_radius = 2.75;  //radius of reference wheel
		
	float v_l; //  left side wheel speed inches per second
	float v_r;//  right side wheel speed ips
	float v_rref;// right reference wheel speed ips
	float v_lref;// left reference wheel speed ips
	float l_rate_rps;  //left wheel speed rps      
	float r_rate_rps;  //right wheel speed rps
	float dist_l;
	float dist_r;
	float dist_lref;
	float dist_rref;
	                   
	float leftCmd_limit_pos;
	float rightCmd_limit_pos;
	float leftCmd_limit_neg;
	float rightCmd_limit_neg;
		
	float v_avg;
	
	float heading;
	float headingRate;  //for the gyro
		
	float accelerationX;
	float accelerationY;
	float velocityX;
	float velocityY;	
	*/
	/**
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.
	 */
	Neurologist::Neurologist(void): 
		updateTask((char *)"Update", (FUNCPTR)Update, 75)
	  
	{
		GetWatchdog().SetExpiration(100);
		
		ds = DriverStation::GetInstance();
		dsLCD = DriverStationLCD::GetInstance();
		//myRobot = new RobotDrive(1, 2, 0.5);	// robot will use PWM 1-2 for drive motors
		driveStick = new Joystick(1);			// create the joysticks
		liftStick = new Joystick(2);
		
		time = new RobodoxTimer;
		
		maxWheelRate = (12/Ke)/gearRatio;		//Ke is in volts/(radian/second)
		
		horizontalServo = new Servo(4,10); 		// create horizontal servo
		verticalServo = new Servo(4,9);			// create vertical servo
		
		leftMotor = new Jaguar(4,1);
		rightMotor = new Jaguar(4,2);
		liftR = new Relay(4,1);
		liftL = new Relay(4,2);
		agitator = new Relay(4,3);
		rollerL = new Victor(4,3);
		rollerR = new Victor (4,4);
		
		v_battery = 12;
		
		v_l = 0;   //  left side wheel speed inches per second
		v_r = 0;  //  right side wheel speed ips
		v_rref = 0.;// right reference wheel speed ips
		v_lref = 0.;// left reference wheel speed ips
		l_rate_rps = 0.;  //left wheel speed rps      
		r_rate_rps = 0.;  //right wheel speed rps
		dist_l = 0.;
		dist_r = 0.;
		
		deadzone = 0.039;
	    slope = 1.04;
	    intercept = 0.041;
		
		g1 = new Gyro(1);
		g1->SetSensitivity(.007); // measured in volts/degree/second
		
		
		/*leftGearbox = new Encoder(4,1,4,2, false, Encoder::k1X);	  //changed from leftfront
		rightGearbox = new Encoder (4,3,4,4, false, Encoder::k1X);  //changed from rightfront
		rightRef = new Encoder (4,5,4,6, false, Encoder::k1X);
		leftRef = new Encoder (4,7,4,8, false, Encoder::k1X);
		*/
		
		leftGearbox = new RobodoxEncoder(4,1,4,2, false, RobodoxEncoder::k1X);	  //changed from leftfront
		rightGearbox = new RobodoxEncoder (4,3,4,4, false, RobodoxEncoder::k1X);  //changed from rightfront
		rightRef = new RobodoxEncoder (4,5,4,6, false, RobodoxEncoder::k1X);
		leftRef = new RobodoxEncoder (4,7,4,8, false, RobodoxEncoder::k1X);
		
		leftRef->SetDistancePerPulse((3.0*PI)/250);
		rightRef->SetDistancePerPulse((3.0*PI)/250);
		leftGearbox->SetDistancePerPulse((6.0*PI)/250);
		rightGearbox->SetDistancePerPulse((6.0*PI)/250);
		//leftGearbox->SetMinRate (1.);//sets minimum rate to 1 in/sec so it doesn't give infinity errors
		//rightGearbox->SetMinRate (1.);
		//rightRef->SetMinRate(1.);
		//leftRef->SetMinRate(1.);
		//leftGearbox->SetMaxPeriod(1.);
		//rightGearbox->SetMaxPeriod(1.);
		rightRef->SetReverseDirection(true);
		rightGearbox->SetReverseDirection(true);
		
		leftRef->Start();
		rightRef->Start();
		leftGearbox->Start();
		rightGearbox->Start();
		
		time->Start();
		
		a_x = new Accelerometer(2,1); //possibly remove later 
		a_y = new Accelerometer(2,2);
		//a_z = new Accelerometer(2,3);
		a_x->SetSensitivity(300); // millivolts/g
		a_y->SetSensitivity(300); // millivolts/g
		
		toplimitswitch = new DigitalInput(4,9);
		bottomlimitswitch = new DigitalInput(4,10);
		
		autoswitch1 = new DigitalInput(4,11);
		autoswitch2 = new DigitalInput(4,12);
		
		leftMotorCmd = 0.0;
		rightMotorCmd = 0.0;
		lastLeftMotorCmd = 0.0;
		lastRightMotorCmd= 0.0;
		
		Mr_Robauto = new NeuroAuto(leftMotor, rightMotor, leftGearbox, rightGearbox, rightRef, leftRef, time, g1);  //YAP: took out left/right rear
		
		servoDeadband = 0.01;					// move if > this amount 
		framesPerSecond = 15;					// number of camera frames to get per second
		sinStart = 0.0;							// control where to start the sine wave for pan
		memset(&par1,0,sizeof(ParticleAnalysisReport));			// initialize particle analysis report
		memset(&par2,0,sizeof(ParticleAnalysisReport));			// initialize particle analysis report

		/* image data for tracking - override default parameters if needed */
		/* recommend making PINK the first color because GREEN is more 
		 * subsceptible to hue variations due to lighting type so may
		 * result in false positives */
		// PIN				
		sprintf (td1.name, "PINK");
		td1.hue.minValue = 220;   
		td1.hue.maxValue = 255;  
		td1.saturation.minValue = 75;   
		td1.saturation.maxValue = 255;      
		td1.luminance.minValue = 85;  
		td1.luminance.maxValue = 255;
		// GREEN
		sprintf (td2.name, "GREEN");
		td2.hue.minValue = 55;   
		td2.hue.maxValue = 125;  
		td2.saturation.minValue = 58;   
		td2.saturation.maxValue = 255;    
		td2.luminance.minValue = 92;  
		td2.luminance.maxValue = 255;
				
		/* set up debug output: 
		 * DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE 
		*/
		SetDebugFlag(DEBUG_SCREEN_ONLY);
				
		/* start the CameraTask	 */
		if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1) {
			DPRINTF(LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s", GetVisionErrorText(GetLastVisionError()) );
		}
		/* allow writing to vxWorks target */
		Priv_SetWriteFileAllowed(1);   		

		/* stop the watchdog if debugging  */
		GetWatchdog().SetExpiration(0.5);
		GetWatchdog().SetEnabled(false);		
		
		//updateTask.Start((INT32)this);
			
		if (!updateTask.Start((INT32)this)){
			//wpi_fatal(UpdateTaskError);
			printf("UpdateTask failed to start\r\n");
		}
	}

	/**
	 * Set servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0). 
	 * 
	 * @param normalizedHorizontal Pan Position from -1.0 to 1.0.
	 * @param normalizedVertical Tilt Position from -1.0 to 1.0.
	 */
	void Neurologist::setServoPositions(float normalizedHorizontal, float normalizedVertical)	{

		float servoH = NormalizeToRange(normalizedHorizontal);
		float servoV = NormalizeToRange(normalizedVertical);
				
		float currentH = horizontalServo->Get();		
		float currentV = verticalServo->Get();
				
		/* make sure the movement isn't too small */
		if ( fabs(servoH - currentH) > servoDeadband ) {
			horizontalServo->Set( servoH );
			/* save new normalized horizontal position */
			horizontalPosition = RangeToNormalized(servoH, 1);
		}
		if ( fabs(servoV - currentV) > servoDeadband ) {
			verticalServo->Set( servoV );
			verticalPosition = RangeToNormalized(servoV, 1);
		}
	}

	/**
		 * Adjust servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0). 
		 * 
		 * @param normalizedHorizontal Pan adjustment from -1.0 to 1.0.
		 * @param normalizedVertical Tilt adjustment from -1.0 to 1.0.
		 */
	void Neurologist::adjustServoPositions(float normDeltaHorizontal, float normDeltaVertical)	{
							
		/* adjust for the fact that servo overshoots based on image input */
		normDeltaHorizontal /= 8.0;
		normDeltaVertical /= 4.0;
			
		/* compute horizontal goal */
		float currentH = horizontalServo->Get();
		float normCurrentH = RangeToNormalized(currentH, 1);
		float normDestH = normCurrentH + normDeltaHorizontal;	
		/* narrow range keep servo from going too far */
		if (normDestH > 1.0) 
			normDestH = 1.0;
		if (normDestH < -1.0) 
			normDestH = -1.0;			
		/* convert input to servo range */
		float servoH = NormalizeToRange(normDestH);

		/* compute vertical goal */
		float currentV = verticalServo->Get();
		float normCurrentV = RangeToNormalized(currentV, 1);
		float normDestV = normCurrentV + normDeltaVertical;	
		if (normDestV > 1.0) 
			normDestV = 1.0;
		if (normDestV < -1.0) 
			normDestV = -1.0;
		/* convert input to servo range */
		float servoV = NormalizeToRange(normDestV, 0.2, 0.8);

		/* make sure the movement isn't too small */
		if ( fabs(currentH-servoH) > servoDeadband ) {
			horizontalServo->Set( servoH );		
			/* save new normalized horizontal position */
			horizontalPosition = RangeToNormalized(servoH, 1);
		}			
		if ( fabs(currentV-servoV) > servoDeadband ) {
			verticalServo->Set( servoV );
			verticalPosition = RangeToNormalized(servoV, 1);
		}
	}		
	
	// JDM
	// Turn robot so that camera is straight and drive towards target
	// Left motor works fine, right chatters back and forth
	void Neurologist::CameraDrive(float servo) { 
		float left_motor, right_motor;
		float gain = 0.5;
		
		left_motor = leftMotor->Get();
		right_motor = rightMotor->Get();
		
		//servo = servo*2 - 1.0;
		
		if(servo < 0) { //moving to the right
			
			left_motor += gain*servo;
		    right_motor += gain*servo;  //assuming right motor reversed 
		}
		
		else {  //moving to the left
			left_motor -= gain*servo;
			right_motor -= gain*servo;  //assuming right motor reversed
		}	
		
		//ShowActivity("Left = %f\t Right = %f", left_motor, right_motor);  //YULIYA: CHECK, IMPORTANT!!!!
		
		//myRobot->SetLeftRightMotorSpeeds(left_motor, right_motor);
		leftMotor->Set(left_motor);
		rightMotor->Set(right_motor);
	}
	
	
	void Neurologist::Autonomous(void) {
		DPRINTF(LOG_DEBUG, "Autonomous");				
				
		DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name);
				
		// initialize position and destination variables
		// position settings range from -1 to 1
		// setServoPositions is a wrapper that handles the conversion to range for servo 
		horizontalDestination = 0.0;		// final destination range -1.0 to +1.0
		verticalDestination = 0.0;
				
		// initialize pan variables
		// incremental tasking toward dest (-1.0 to 1.0)
		float incrementH, incrementV;
		// pan needs a 1-up number for each call
		int panIncrement = 0;							
				
		// current position range -1.0 to +1.0
		horizontalPosition = RangeToNormalized(horizontalServo->Get(),1);	
		verticalPosition = RangeToNormalized(verticalServo->Get(),1);			
				
		// set servos to start at center position
		setServoPositions(horizontalDestination, verticalDestination);

		// for controlling loop execution time 
		float loopTime = 0.1;		
		//float loopTime = 0.05;											
		double currentTime = GetTime();
		double lastTime = currentTime;
												
		// search variables 
		bool foundColor = 0; 
		double savedImageTimestamp = 0.0;
		bool staleImage = false; 
		

		
		SecondColorPosition colorPosition;
		// BELOW and ABOVE swapped because camera is now upside down		
		if(ds->GetAlliance()==DriverStation::kRed)  //red alliance = pink above green
			colorPosition = BELOW;
		else if(ds->GetAlliance()==DriverStation::kBlue)  //blue alliance = green above pink
			colorPosition = ABOVE;
		else
			colorPosition = RIGHT;  // random

		/*** TEMPORARY TEST CODE *****/
		colorPosition = ABOVE;
		/*** TEMPORARY TEST CODE *****/
		
		while( IsAutonomous() )	{
			GetWatchdog().Feed();		// turn watchdog off while debugging	
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "h=%f v=%f", horizontalServo->Get(), verticalServo->Get());
			dsLCD->UpdateLCD();
			
			if(autoswitch1->Get() == 0 && autoswitch2->Get() == 1)
				Mr_Robauto->RandomDrive();
			else if(autoswitch1->Get() == 0 && autoswitch2->Get() == 0)
				Mr_Robauto->CenterCircling();
			else if(autoswitch1->Get() == 1 && autoswitch2->Get() == 0)
				Mr_Robauto->CornerCircling();		
			else {
				
				// calculate gimbal position based on colors found 
				if ( FindTwoColors(td1, td2, colorPosition, &par1, &par2) ){
					//PrintReport(&par2);
					foundColor = true;
					// reset pan		
					panIncrement = 0;  		
					if (par1.imageTimestamp == savedImageTimestamp) {
						// This image has been processed already, 
						// so don't do anything for this loop 
						staleImage = true;
						DPRINTF(LOG_DEBUG, "STALE IMAGE");
					
					} 
					else {
						// The target was recognized
						// save the timestamp
						staleImage = false;
						savedImageTimestamp = par1.imageTimestamp;	
						DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp);

						// Here is where your game-specific code goes
						// when you recognize the target
					
						// get center of target 
						// Average the color two particles to get center x & y of combined target
						horizontalDestination = (par1.center_mass_x_normalized + par2.center_mass_x_normalized) / 2;	
						verticalDestination = (par1.center_mass_y_normalized + par2.center_mass_y_normalized) / 2;							
					}
				} 
				else   // need to pan 
					foundColor = false;
			 
								
				if(foundColor && !staleImage) {	
					/* Move the servo a bit each loop toward the destination.
					 * Alternative ways to task servos are to move immediately vs.
					 * incrementally toward the final destination. Incremental method
					 * reduces the need for calibration of the servo movement while
					 * moving toward the target.
					 */
					incrementH = (horizontalDestination - horizontalPosition);  // made negative because of upside down servo
					// you may need to reverse this based on your vertical servo installation
					//incrementV = verticalPosition - verticalDestination; //use this one b/c camera upside down
					incrementV = verticalDestination - verticalPosition;
					adjustServoPositions( incrementH, incrementV );  
					
					CameraDrive(horizontalDestination); // JDM
					
					ShowActivity ("** %s & %s found: Servo: x: %f  y: %f ** ", 
							td1.name, td2.name, horizontalDestination, verticalDestination);	
					
				} 
				else { //if (!staleImage) {  // new image, but didn't find two colors
					
					leftMotor->Set(0.0);
					rightMotor->Set(0.0);
					
					// adjust sine wave for panning based on last movement direction
					if(horizontalDestination > 0.0)	
						sinStart = PI/2.0; 
					else 
						sinStart = -PI/2.0; 
					
					/* pan to find color after a short wait to settle servos
					 * panning must start directly after panInit or timing will be off */				
					if (panIncrement == 3) {
						panInit(8.0);		// number of seconds for a pan
					}
					else if (panIncrement > 3) {					
						panForTarget(horizontalServo, sinStart);	
						
						/* Vertical action: In case the vertical servo is pointed off center,
						 * center the vertical after several loops searching */
						if (panIncrement == 20) { verticalServo->Set( 0.5 );	}
					}
					panIncrement++;		
					
				}  // end if found color
				if( horizontalDestination >= 0.5){
					ds->SetDigitalOut(1,true);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,false);
					printf("Left\r\n");
				}
							
				else if( (horizontalDestination > -0.5) && 
					 (horizontalDestination < 0.5)){
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,true);
					ds->SetDigitalOut(3,false);
					printf("Center\r\n");
				}
							
				else if( horizontalDestination <= -0.5){
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,true);
					printf("Right\r\n");
				}
				else {
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,false);
					printf("None\r\n");
				}			
				
				// sleep to keep loop at constant rate
				// this helps keep pan consistant
				// elapsed time can vary significantly due to debug printoutg
				currentTime = GetTime();			
				lastTime = currentTime;					
				if ( loopTime > ElapsedTime(lastTime) ) {
					Wait( loopTime - ElapsedTime(lastTime) );	// seconds
				}			
				
			} // end if(0)
		}  // end while
		
		//leftMotor->Set(0.0);
		//rightMotor->Set(0.0);
		
		DPRINTF(LOG_DEBUG, "end Autonomous");
		ShowActivity ("Autonomous end");
	}  // end autonomous

	void Neurologist::OperatorControl(void)  {
		//char funcName[]="OperatorControl";
		DPRINTF(LOG_DEBUG, "OperatorControl");

		printf("time, left distance, left rate, right distance, right rate");
		GetWatchdog().SetEnabled(true);

		bool foundColor = false; 
		bool staleImage = true;
		double savedImageTimestamp = 0.0;
		
		while ( IsOperatorControl() )
		{
			GetWatchdog().Feed();

			DriveCode(); 

			LiftCode();
			RollerCode();
						
			verticalServo->Set(0.5);
			horizontalServo->Set(0.5);

			if ( FindTwoColors(td1, td2, ABOVE, &par1, &par2) ){
				//PrintReport(&par2);
				foundColor = true;
				
				if (par1.imageTimestamp == savedImageTimestamp) {
					// This image has been processed already, 
					// so don't do anything for this loop 
					staleImage = true;
					DPRINTF(LOG_DEBUG, "STALE IMAGE");
					
				} 
				else {
					// The target was recognized
					// save the timestamp
					staleImage = false;
					savedImageTimestamp = par1.imageTimestamp;	
					DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp);
				
					// Here is where your game-specific code goes
					// when you recognize the target
								
					// get center of target 
					// Average the color two particles to get center x & y of combined target
					horizontalDestination = (par1.center_mass_x_normalized + par2.center_mass_x_normalized) / 2;							
				}
			} 
			else   // need to pan 
				foundColor = false;

			if(foundColor){	
				if( horizontalDestination >= 0.5){
					ds->SetDigitalOut(1,true);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,false);
					printf("Left\r\n");
				}
						
				else if( horizontalDestination > -0.5 && 
						 horizontalDestination < 0.5){
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,true);
					ds->SetDigitalOut(3,false);
					printf("Center\r\n");
				}
						
				else if( horizontalDestination <= -0.5){
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,true);
					printf("Right\r\n");
				}
			
				else {
					ds->SetDigitalOut(1,false);
					ds->SetDigitalOut(2,false);
					ds->SetDigitalOut(3,false);
					printf("None\r\n");
				}	
			}
			
			//ShowActivity("top switch = %d\t bottom switch = %d\t", toplimitswitch->Get(), bottomlimitswitch->Get());
			//dsLCD->Printf(DriverStationLCD::kMain_Line6, 1, "throttle=%f", .25*(1-(driveStick->GetThrottle())));
			//dsLCD->Printf("DS Dig 1 = %d Dig 2 = %d", ds->GetDigitalIn(1), ds->GetDigitalIn(2));
			//dsLCD->Printf(DriverStationLCD::kMain_Line6, 1, "topswitch = %d bottom = %d", toplimitswitch->Get(), bottomlimitswitch->Get());
			//printf("%f\t %f\t %f\t %f\t\n", (float)time->Get(),(float)leftGearbox->GetDistance(), leftGearbox->GetRate(), leftGearbox->GetRate(.25));
			//printf("lifty = %f\t drivey = %f\t\n", liftStick->GetY(), driveStick->GetY());
			//printf("throttle = %f\t\n", driveStick->GetThrottle());
			//printf("Y-axis = %f\t leftMotorCommand = %f\t lastLeftMotorComamnd = %f\r\n", driveStick->GetY(), leftMotorCmd, lastLeftMotorCmd);  
			dsLCD->UpdateLCD();
			printf("%f\t %f\t\n", (float)rightGearbox->GetDistance(), rightGearbox->GetRate());
			//printf("horservo = %f\t vertservo = %f\t\n", horizontalServo->Get(), verticalServo->Get());
		}  
	} // end operator control	
	
	
/*private:
	
	float deadzone;
	float slope;
	float intercept;
	float rollerlast;*/
	
	void Neurologist::DriveCode() {
		//float cmd_limit = .25*(1-(driveStick->GetThrottle()));// cut the sensitivity down by factor of 5 
		GetStickData();
		//leftMotorCmd=TorqueLimiter(leftMotorCmd, lastLeftMotorCmd, cmd_limit, 1);
		//rightMotorCmd=TorqueLimiter(rightMotorCmd, lastRightMotorCmd, cmd_limit, 1);
		/* traction control */
	  /*	if(time->Get()>=50){
			TractionControl();
		} */
		
		if(ds->GetDigitalIn(1)){
			StepCmd();
		}
 
		TractionControl();
		SetMotorData();
		
	}

	void Neurologist::SetMotorData(){
		//myRobot->SetLeftRightMotorSpeeds(yt+xt, yt-xt);  // swapped plus & minus (used for RobotDrive)
		leftMotor->Set(leftMotorCmd);
		rightMotor->Set(rightMotorCmd);  //right motor is reversed
	}
	
	void Neurologist::GetStickData(){ 
		lastRightMotorCmd = rightMotorCmd;
		lastLeftMotorCmd = leftMotorCmd;
		
		
		 if(driveStick->GetX() > -deadzone  && driveStick->GetX() < deadzone) 
			xt = 0;
							
	    else if(driveStick->GetX() >= deadzone )
			xt = slope*driveStick->GetX() - intercept;
					
	    else
		    xt = slope*driveStick->GetX() + intercept;

		if((-driveStick->GetY()) > -deadzone  && (-driveStick->GetY()) < deadzone )
			yt = 0;
							
		else if((-driveStick->GetY()) >= deadzone )
			yt = slope*(-driveStick->GetY()) - intercept;
							
		else
			yt = slope*(-driveStick->GetY()) + intercept;
			
		if(xt>0)
			xt= (xt*xt);
		else
		    xt= -(xt*xt);
		    
		if(yt>0)
			yt=yt*yt;
		else
			yt= -(yt*yt);
			
		leftMotorCmd = xt+yt;
		rightMotorCmd = xt-yt;				
		
	}
	//if(using traction control) use this one
	float Neurologist::TorqueLimiter(float input_cmd, float motor_rate_norm , float cmd_limit_pos, float cmd_limit_neg, float ratio){
			// This function uses motor date independent of a reference wheel to limit torque
			// input_cmd = motor command [-1,1] 
			// motor_rate_norm = normalized motor rate [-1,1]   This is the motor speed / max motor speed(corrected for voltage variation)
			//cmd_limit_pos  = motor torque limit (normalized units) in forward direction
			//cmd_limit_neg  = motor torque limit (normalized units) in aft direction   Note; this is specified as a positive number even though it limits neg values
			//limited_cmd  = the value of the command that is restriced to keep torque below the limit

		    float limited_cmd ;
			
			cmd_limit_pos = cmd_limit_pos*ratio;
			cmd_limit_neg = cmd_limit_neg*ratio;
			if( (input_cmd-motor_rate_norm) > cmd_limit_pos ){
				limited_cmd = motor_rate_norm + cmd_limit_pos  ;
			}
			else  if( (input_cmd-motor_rate_norm) < -cmd_limit_neg ){
				limited_cmd = motor_rate_norm - cmd_limit_neg  ;
			}
			else{
				limited_cmd = input_cmd;
			}
			return limited_cmd ;
		}//end TorqueLimiter routine
	

			/****************************************************************************************************************************
			 * Revised TractionControl for 2/15/2009 code																					*
			 * Routine reads the driveStick Throttle, scales it and uses it to set torque limits
			 * The torque limit will by bypassed by setting the throttle > 90%ss the torqu limiter. This is like a switch.
			 * left and right limits are set by holding button 8 or 9 down and adjusting the throttle until the wheels slip
			 * if buttons aren't pressed, then the limits should default to the global init values
			 * **************** bool sw_encoders_in
			 * The limits are either used with encoder feedback  ie sw_encoders_in=true;
			 * or backup command rate limits using lastMotorCmds (sw_encoders_in = false;
			 * If using ecoder rate, then the driver can hold max stick and adjust the limits. 
			 * If using lastCmdlimits, then best to pulse the stick for about a second.
			 * **************** bool sw_compensate_battery
			 * The limits are adjusted for battery voltage.  
			 * This hasnt been verified so a sw_compensate_battery bool can be used to remove it.
			 * ****************
			 * v_l and v_r are now local variables 
			 * 
			 * TODO the encoders SetMinRate(1.) are done here but should be moved up in constructor
			 * TODO lastLeftMotorCmd make sure these are spelled right in this routine
			 * TODO Verify that Button 8 is left and button 9 is right side of stick.
			 ****************************************************************************************************************************/
			 void Neurologist::TractionControl (){
				static float v_max = 136.2;//max wheel speed at 12 volts 
				static float l_ratio= 1; // these will not change for now
				static float r_ratio= 1; //  these will not change for now
				static float driverSetLimit=.5;//this is the driver throttle input min 0, max .5
				static float battery_factor =1.;
				static float v_l = 0.;
				static float v_r = 0.;
				static float v_lref = 0.;
				static float v_rref = 0.;
				//static float v_r_slip = 0.;
				//static float v_l_slip = 0.;
				static bool  sw_encoders_in = true;//TODO maybe move to parameter control area
				static bool  sw_compensate_battery = true;//TODO maybe move to parameter control area
				//static bool  sw_use_ref_wheel = false;
				static float leftCmd_limit_pos = 1.;
				static float leftCmd_limit_neg = 1.;
				static float rightCmd_limit_pos = 1.;
				static float rightCmd_limit_neg = 1.;
				//static float tol = 2.;
				//static float tq_limit_ratio = .8; //reduces torque ,imit by this when we are slipping
				//read the throttle position and modifiy it to read between 0 and .5
				//should read 0 when the throttle is all the way down.
				driverSetLimit= .25*(1.-driveStick->GetThrottle());
			 
				//hold the left button 8 down to set the left torque limit with throttle
			 
				if(driveStick->GetRawButton(8)) {
					Wait(.3); //wait .3 second and if button is still on then set limits
					if(driveStick->GetRawButton(8)) {//i assumed that this is left TODO check
					leftCmd_limit_pos = driverSetLimit; 
					leftCmd_limit_neg=  driverSetLimit;
					}
				}
				//hold the right button 9 down to set the right torque limit with throttle
				//note: can do both at once if you want to set them equal
				if(driveStick->GetRawButton(9)){
					Wait(.3); //wait .3 second and if button is still on then set limits
					if(driveStick->GetRawButton(9)) {
					rightCmd_limit_pos = driverSetLimit; 
					rightCmd_limit_neg=  driverSetLimit;
					}
				}
			 
				//If throttle is > .45 then assume we want to turn off torque limits else enter loop
				if(driverSetLimit<.45) { 
				 
	
			 
				//read the wheel speeds
				v_l=leftGearbox->GetRate();
				v_r=rightGearbox->GetRate();
				//printf("%f\t %f\r\n", v_l, v_r);
				if(v_l> 1200 || v_l< -1200.){v_l=0.;}//this gets rid of inf rate problem
				if(v_r> 1200 || v_r< -1200.){v_r=0.;}
				//if(v_r == NaN || v_r == -NaN) {v_r=0.;}
				//if(v_r == NaN || v_r == -NaN) {v_r=0.;}
			    v_lref = leftRef->GetRate();
			    v_rref = rightRef->GetRate();
				if(v_lref> 1200 || v_lref< -1200.){v_lref=0.;}
				if(v_rref> 1200 || v_rref< -1200.){v_rref=0.;}
				//printf(" a %f\t %f\r\n", v_l, v_r);
				
				//left wheel slipping?
						  
			//	v_l_slip = fabs(v_l) - fabs(v_lref) ;
			//	if(v_l_slip > tol && sw_use_ref_wheel){
			//		l_ratio= tq_limit_ratio;
			//	}
			//	else{
			//		l_ratio = 1;
			//	}
			//	  
			//	//right wheel slipping?
			//	v_r_slip = fabs(fabs(v_r) - fabs(v_rref));
			//	if(v_r_slip > tol  && sw_use_ref_wheel){
			//		r_ratio= tq_limit_ratio;
			//	}
			//  	else{
			//  		r_ratio = 1;
			//  	}
				//see if we want to use the encoders or just rate limit stick as before	 
				if(sw_encoders_in){
					leftMotorCmd  = TorqueLimiter(leftMotorCmd,v_l /v_max, leftCmd_limit_pos, leftCmd_limit_neg,  l_ratio);
					rightMotorCmd = TorqueLimiter(rightMotorCmd, v_r/v_max, rightCmd_limit_pos, rightCmd_limit_neg,  r_ratio);
				}
				else {
					leftMotorCmd  = TorqueLimiter(leftMotorCmd,lastLeftMotorCmd, leftCmd_limit_pos, leftCmd_limit_neg, 1.);
					rightMotorCmd = TorqueLimiter(rightMotorCmd,lastRightMotorCmd, rightCmd_limit_pos, rightCmd_limit_neg, 1.);
				}
				//Compensate command for battery voltage variations 
				if(sw_compensate_battery){
				  float v_battery = ds->GetBatteryVoltage();  //CDS Change this back to ds->GetBatteryVoltage();
					if(v_battery < 10) v_battery = 10; // limit the ammount of correction...just in case the battery voltage goes to zero 
						battery_factor = 12/v_battery ; 
						leftMotorCmd = leftMotorCmd*battery_factor; 
						rightMotorCmd = rightMotorCmd*battery_factor;
				}// end battery compensation test
			    
				} //end if driverSetCmd < .45   if false, the torqlimiter is turned off
				
			}
	
	void Neurologist::LiftCode() {
		static Relay::Value lastMovementValue;       
		
		if(liftStick->GetRawButton(6) && liftStick->GetRawButton(7)){

			if(lastMovementValue == Relay::kForward){
				liftR->Set(Relay::kReverse);
				liftL->Set(Relay::kReverse);
				lastMovementValue = Relay::kReverse;
			}
			
			else if(lastMovementValue == Relay::kReverse){
				liftR->Set(Relay::kForward);
				liftL->Set(Relay::kForward);
				lastMovementValue = Relay::kForward;
			}		
		}
		
		if(liftStick->GetRawButton(6)){   
			liftR->Set(Relay::kForward);
			liftL->Set(Relay::kForward);
			lastMovementValue = Relay::kForward;
		}
		
		else if(liftStick->GetRawButton(7)){ 
			liftR->Set(Relay::kReverse);
			liftL->Set(Relay::kReverse);
			lastMovementValue = Relay::kReverse;
		}
							
	    else{
			liftR->Set(Relay::kOff);
			liftL->Set(Relay::kOff);
	    }
		
		if(toplimitswitch->Get() == 0 && lastMovementValue == Relay::kForward){ // bottom limit 0 = off, 1 = hit 
			liftR->Set(Relay::kOff);   
			liftL->Set(Relay::kOff);
		}
		
		if(bottomlimitswitch->Get() == 1 && lastMovementValue == Relay::kReverse){  // top limit 0 = hit, 1 = off
			liftR->Set(Relay::kOff);
			liftL->Set(Relay::kOff);
		}
	}
	

	void Neurologist::RollerCode() {   

		
		float yt_roller;		
		
		agitator->Set(Relay::kForward);
		
		if(liftStick->GetRawButton(11)) {  //assuming rolling out
			yt_roller = 0.4;
		    agitator->Set(Relay::kReverse); //assuming out
		}
			
		else if(liftStick->GetRawButton(10)) { //assuming rolling in
			yt_roller = -0.3;
			agitator->Set(Relay::kForward); //assuming in
		}
		
		else {
			if((-liftStick->GetY()) > -deadzone  && (-liftStick->GetY()) < deadzone )   //reversing liftstick b/c y-axis on joystick reversed 
				yt_roller = 0;
							
			else if((-liftStick->GetY()) >= deadzone ) { //rolling out
				yt_roller = slope*(-liftStick->GetY()) - intercept;
				agitator->Set(Relay::kReverse);  //rolling out?
		    }
									
			else {
				yt_roller = slope*(-liftStick->GetY()) + intercept; //rolling in
				agitator->Set(Relay::kForward);  //rolling in?
			}
				
			if(yt_roller>0)
				yt_roller = yt_roller*yt_roller;
			else
				yt_roller = -(yt_roller*yt_roller);  //so that sign isn't lost in squaring
		}
		
		rollerL->Set(yt_roller);
		rollerR->Set(yt_roller);
		
	}
	
	/*void Agitate() { 
	    if(liftStick->GetTrigger())   //roller runs in
			agitator->Set(Relay::kForward);		
		else if(liftStick->GetRawButton(3))  //roller runs out
			agitator->Set(Relay::kReverse);
		else
			agitator->Set(Relay::kOff);
	    
	     if(ds->GetDigitalIn(1))  //roller out
	     	agitator->Set(Relay::kReverse);
	       
	       else if(ds->GetDigitalIn(2))   //roller in
	       	agitator->Set(Relay::kForward);
	       	
	       else   
	       	agitator->Set(Relay::kOff);  //off
	}    
	*/
	void Neurologist::StepCmd() {
		static bool printFlag = false;
		static float  startTime= time->Get();
		if(time->Get() == startTime) {
			printf("%f\t %f\t %f\t %f\t %f\t\n",time->Get(), rightGearbox->GetRate(), leftGearbox->GetRate(), leftRef->GetRate(), rightRef->GetRate());		
		}
		else if (time->Get() < startTime+1.) {
			//printf("%f\t %f\t %f\t %f\t\n", rightGearbox->GetRate(), leftGearbox->GetRate(), leftRef->GetRate(), rightRef->GetRate());	
			rightMotorCmd = -1.0;
			leftMotorCmd = 1.0;
		}
		else if (printFlag == false){
			printf("%f\t %f\t %f\t %f\t %f\t\n",time->Get(), rightGearbox->GetRate(), leftGearbox->GetRate(), leftRef->GetRate(), rightRef->GetRate());	
			printFlag = true;
			rightMotorCmd = 0.0;
			leftMotorCmd = 0.0;
		}
	}
//};

// entry point FRC_UserProgram_StartupLibraryInit
START_ROBOT_CLASS(Neurologist);
